#include <iostream>
#include <string.h>

#include <stdlib.h>
#include <stdio.h>

#include <sys/time.h>
#include <unistd.h>
#include <sys/select.h>

#include "basic_utils.hh"
#include "other_functions.h"

#include "model.h"
#include "view.h"

#define UMBRAL_0 10
#define UMBRAL_1 20
#define UMBRAL_2 30

#define UMBRAL_DEFAULT2 UMBRAL_0*UMBRAL_0

const float CModel3D::umbral_dist2[3] = {UMBRAL_0*UMBRAL_0, 
					UMBRAL_1*UMBRAL_1, 
					UMBRAL_2*UMBRAL_2};

void CModel3D::alloc(int points)
{
    npoints = points;
    dealloc();
    m_alloc=true;
    epilines = cvCreateMat (npoints, 3, CV_32FC1);
    mask_show = cvCreateMat(npoints, 1, CV_8UC1);
    points2d = cvCreateMat(npoints, 2, CV_32FC1);
    calcLinesModel();
    CentreModel();    
}

void CModel3D::dealloc()
{
    if (!m_alloc)
	return;
    m_alloc=false;
    cvReleaseMat(&lines);
    cvReleaseMat(&epilines);
    cvReleaseMat(&mask_show);
    cvReleaseMat(&points2d);
    if (!thresholds2)
    cvReleaseMat(&thresholds2);
}

/* Sort points in top-to-bottom order */
int CModel3D::comparex( const void* _a, const void* _b)
{
    float a = ((CvPoint2D32f*)_a)->x;
    float b = ((CvPoint2D32f*)_b)->x;
    
    return a > b;
}


void CModel3D::calcLinesModel(int nlines)
{
    if (!nlines)
	nlines = m_nlines;
    else
    {
	if (m_nlines != 0 && nlines != m_nlines)
	    if (lines)
	    {
		cvReleaseMat(&lines);
		lines=0;
	    }
	m_nlines = nlines;
    }

    if (!lines)
	lines = cvCreateMat (npoints, m_nlines, CV_32FC1);

    int i, j;
    float disttx, distty, disttz;
    CvPoint2D32f *distt = (CvPoint2D32f*)malloc(npoints*sizeof(CvPoint2D32f));

    // para cada punto busca sus dos puntos en xyz más cercanos
    for (i=0; i<npoints; i++)
    {
	for (j=0; j<npoints; j++)
	{
	    if (j==i) 
	    {
		distt[j].x = 10000;
		distt[j].y=j;
		continue;
	    }
	    disttx = cvmGet(points3d, i,0) - cvmGet(points3d, j, 0);
	    disttx *= disttx;
	    distty = cvmGet(points3d, i,1) - cvmGet(points3d, j, 1);
	    distty *= distty;
	    disttz = cvmGet(points3d, i,2) - cvmGet(points3d, j, 2);
	    disttz *= disttz;
	    distt[j].x = sqrt(disttx + distty + disttz);
	    distt[j].y = (float)j;
	}
	qsort(distt, npoints, sizeof(CvPoint2D32f), comparex);
	
	for (j=0; j<m_nlines; j++)
	{
	    cvmSet(lines, i, j, distt[j].y);
	}
    }

    free(distt);
}	

CvScalar CModel3D::CentreModel()
{
    CvMat  points3D2;
    cvReshape(points3d, &points3D2, 3, 0);
    centroXYZ = cvGet1D(&points3D2, 0);
    cvSubS(&points3D2, centroXYZ, &points3D2);
    
    return centroXYZ;
}

int CModel3D::ReadModelFromFile(const std::string filename)
{
    CvFileStorage* fs = cvOpenFileStorage( filename.c_str(), 0, CV_STORAGE_READ );
    if (fs == NULL) //File is not valid
	return 0;

    points3d = (CvMat *) cvReadByName(fs, NULL, "points3D");
    angY = (CvMat *) cvReadByName(fs, NULL, "angY");
    //convert angY to radians
    cvScale(angY, angY, CV_PI/180.0);

    CvFileNode * node;
    node = cvGetFileNodeByName(fs, NULL, "thresholds2");
    if (node == NULL)
    {
	//this an old .dat file, and doesn't have threshold information
	//use default threshold
	thresholds2 = cvCreateMat(points3d->rows, 1, CV_32FC1);
	cvSet (thresholds2, cvScalar(UMBRAL_DEFAULT2));
    }
    else
    {
	//read threshold2 matrix from file
	thresholds2 = (CvMat*) cvReadByName(fs, NULL, "thresholds2");
    }
    cvReleaseFileStorage( &fs );

    alloc(points3d->rows);
    return npoints;
}


void CModel3D::SaveModel(const std::string filename)
{
    CvFileStorage * fs = cvOpenFileStorage( filename.c_str(), 0, CV_STORAGE_WRITE );
    SaveModel(fs);
    cvReleaseFileStorage(&fs);
}


void CModel3D::SaveModel(CvFileStorage * fs)
{
    CvMat *angY2;
    
    cvWrite(fs, "points3D", points3d);
    angY2 = cvCloneMat(angY);
    cvScale(angY2,angY2, 180.0F/CV_PI);
    cvWrite(fs, "angY", angY2);
    cvWrite(fs, "thresholds2", thresholds2);
    cvReleaseMat(&angY2);
}


int CModel3D::deletepoint(int index)
{

    DeleteRow(&points3d, index );
    DeleteRow(&points2d, index );
    DeleteRow(&epilines, index);
    DeleteRow(&lines, index);
    DeleteRow(&angY, index);
    DeleteRow(&mask_show, index);
    DeleteRow(&thresholds2, index);

    npoints --;
    calcLinesModel();
    return npoints;
}




int CModel3D::addpoint(CvMat *R)
{
    //change size of matrices
    AddRow2new(&points3d);
    AddRow2new(&lines);
    AddRow2new(&epilines);
    AddRow2new(&mask_show);
    AddRow2new(&angY);
    AddRow2new(&points2d);
    AddRow2new(&thresholds2);
    //add default threshold to new points
    cvmSet(thresholds2, npoints, 0, UMBRAL_DEFAULT2);

    cvmSet(points3d, npoints, 0, -100);
    cvmSet(points3d, npoints, 1, -100);
    cvmSet(points3d, npoints, 2, -100);

    return npoints++;
}    

